#include "Arduino.h"
#include "MotionSensor.h"

#include "definitions.h"

MotionSensor::MotionSensor()
{
  // intentionally left blank
}

void MotionSensor::init()
{

  _axPin = ADXL335_AX_ANALOG_IN_PIN;
  _ayPin = ADXL335_AY_ANALOG_IN_PIN;
  _azPin = ADXL335_AZ_ANALOG_IN_PIN;
  _orientations = "0000000000";
  _aggI         = 0;
  _prevX        = 0;
  _prevY        = 0;
  _prevZ        = 0;
  _moved        = false;
  for (int i=0; i<10; i++) { _second[i] = 0; }
}

void MotionSensor::prepare()
{
init();
  // Gjør en del oppdateringer bare for å være sikker på at aksellerometeret er nullstilt før vi begynner å bruke det
  //Serial.print("Prepare...");
  for (int i=0; i<2000; i++) {
    update();
  }
  //Serial.println("Ferdig med prepare");
}

int MotionSensor::getLastG()     { return _lastG; }
int MotionSensor::getDelta()     { return _delta; }
int MotionSensor::getDeltaX()    { return _dx; }
int MotionSensor::getDeltaY()    { return _dy; }
int MotionSensor::getDeltaZ()    { return _dz; }

int MotionSensor::getMaxSecond() { 
	int max = 0;
	for (int i=0; i<10; i++) {
		int temp = abs(_second[i]);
		if (temp > max) { max = temp; }
	}
	return max;
}


int MotionSensor::getAggregate(int deciSecs) {
	int agg = 0;
	deciSecs = constrain(deciSecs, 1, 10);
	for (int i=9; i>=10-deciSecs; i--) {
		int temp = abs(_second[i]);
		agg += temp;
	}
	return agg;
}

void MotionSensor::update() 
{
  if (millis() - _lastUpdate < 1) { return; } // Bare oppdatere et gitt antall ganger i sekundet
  
  // Oppdatere akselerasjonsdata
  updateA();
  
  _lastUpdate = millis();
}

void MotionSensor::updateA() {
  int ax = map(analogRead(_axPin), 0, 675, -512, 512) ;
  int ay = map(analogRead(_ayPin), 0, 675, -512, 512);
  int az = map(analogRead(_azPin), 0, 675, -512, 512);
  
 	aggregateAccelleration(ax, ay, az);
 	if (millis() - _lastAverage < 10) { return; }
  
  ax = _aggX / _numAggregates;
  ay = _aggY / _numAggregates;
  az = _aggZ / _numAggregates;
	resetAggregate();
  
  _dx = ax - _prevX;
  _dy = ay - _prevY;
  _dz = az - _prevZ;
  
  // Finne ut hvor vi beveger oss
  char current   = '0'; // 0 betyr ingen bevegelse
  char down      = '0'; // 0 betyr at ingen er ned i.e. "vektløs"
  int  lastDelta = 15;  // I utgangspunktet må en aksellerasjon være høyere enn dette for å overkomme "vektløsheten"
  int  lastDown  = 75;  // Har noen av sidene en aksellerasjon på over dette er de "ned" 


  //Desse må kalibererst pr kube

switch (KUBETYPE){	
	case 0: //Første kubar

	if (abs(_dx) > lastDelta) { current = _dx > 0 ? 'W' : 'E'; lastDelta = abs(_dx); }
	if (abs(_dy) > lastDelta) { current = _dy > 0 ? 'U' : 'D'; lastDelta = abs(_dy); }
	if (abs(_dz) > lastDelta) { current = _dz > 0 ? 'N' : 'S'; lastDelta = abs(_dz); }

	if (abs(ax) > lastDown) { down = ax > 0 ? 'W' : 'E'; lastDown = abs(ax); }
	if (abs(ay) > lastDown) { down = ay > 0 ? 'U' : 'D'; lastDown = abs(ay); }
	if (abs(az) > lastDown) { down = az > 0 ? 'N' : 'S'; lastDown = abs(az); }  
	break;


	case 1:
//Kretskortmontert kube har desse verdiene (x og y aksen har byttet plass)
	if (abs(_dx) > lastDelta) { current = _dx > 0 ? 'S' : 'N'; lastDelta = abs(_dx); }
	if (abs(_dy) > lastDelta) { current = _dy > 0 ? 'W' : 'E'; lastDelta = abs(_dy); }
	if (abs(_dz) > lastDelta) { current = _dz > 0 ? 'D' : 'U'; lastDelta = abs(_dz); }

	if (abs(ax) > lastDown) { down = ax > 0 ? 'S' : 'N'; lastDown = abs(ax); }
	if (abs(ay) > lastDown) { down = ay > 0 ? 'W' : 'E'; lastDown = abs(ay); }
	if (abs(az) > lastDown) { down = az > 0 ? 'D' : 'U'; lastDown = abs(az); }  

}

  int thisG = abs(lastDown);
  
  _delta    = max(max(_dx, _dy), _dz);
  _moved    = lastDelta > 50;
  _lastG    = thisG;
  _downside = down;
  
  _aggD[_aggI] = current;
  _aggA[_aggI] = _delta;
  _aggI++;
  
  _prevX = ax;
  _prevY = ay;
  _prevZ = az;
  
  /*Serial.print("X: ");
  Serial.print(abs(dx));
  Serial.print(", Y: ");
  Serial.print(abs(dy));
  Serial.print(", Z: ");
  Serial.println(abs(dz));*/
  
  if (_aggI < 10) { return; } // Bare oppdatere 10 ganger i sekundet, slik at _second inneholder det siste sekundets bevegelser
  
  
  
  for (int i=0; i<9; i++) {
    _orientations.setCharAt(i, _orientations.charAt(i+1));
    //_motions.setCharAt(i, _motions.charAt(i+1));
    _second[i] = _second[i+1];
  }
  
  _second[9] = _delta;
  
  char direction = '0';
  int  maxA      = 0;
  for (int i=0; i<10; i++) {
  	if (_aggA[i] > maxA) {
  	  direction = _aggD[i];
  	  maxA = _aggA[i];
  	}
  }
  
  _orientations.setCharAt(9, direction);
 // _motions.setCharAt(9     , maxA > 50 ? 'H' : 'L');

  _aggI = 0;
}

void MotionSensor::aggregateAccelleration(int x, int y, int z) {
	_aggX += x;
	_aggY += y;
	_aggZ += z;
	_numAggregates++;
}

void MotionSensor::resetAggregate() {
 	_aggX = _aggY = _aggZ = _numAggregates = 0;
  _lastAverage = millis();
}

String MotionSensor::getOrientations() {
  return _orientations;
}

char MotionSensor::getDownside() {
  return _downside;
}

boolean MotionSensor::hasMoved() {
  return _moved;
}

void MotionSensor::printRawXYZacceleration(){ 
  Serial.print("X: ");
  Serial.print(analogRead(_axPin), DEC);

  Serial.print(" Y: ");
  Serial.print (analogRead(_ayPin), DEC);
  
  Serial.print(" Z: ");
  Serial.println(analogRead(_azPin), DEC);  
}

